#include <mc_ros/sensors/sensor_handle.h>


// 定义订阅者模版
// 参考 https://gitee.com/caojiangwei/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node.cc
template <typename MessageType>
ros::Subscriber SubscribeWithHandler(
    // 定义函数指针
    void (SensorHandle::*handler)(SensorParams, const typename MessageType::ConstPtr&),
    // 参数传递
    SensorParams sensor_params, ::ros::NodeHandle* const node_handle, SensorHandle* const sensor_handle) 
{
    // 构造订阅者，并返回
    return node_handle->subscribe<MessageType>(sensor_params.sensor_base.topic_name, 10,
        // 定义处理函数，并传参
        boost::function<void(const typename MessageType::ConstPtr&)>(
            [sensor_handle, handler, sensor_params](const typename MessageType::ConstPtr& msg) { 
                (sensor_handle->*handler)(sensor_params, msg);
            }
        )
    );
};


template <typename MT>
image_transport::Subscriber ImageSubscribeWithHandler(
    // 定义函数指针
    void (SensorHandle::*handler)(SensorParams, const typename MT::ConstPtr&),
    // 参数传递
    SensorParams sensor_params, image_transport::ImageTransport* const it, SensorHandle* const sensor_handle) 
{
    // 构造订阅者，并返回
    return it->subscribe(sensor_params.sensor_base.topic_name, 1,
        // 定义处理函数，并传参
        boost::function<void(const typename MT::ConstPtr&)>(
            [sensor_handle, handler, sensor_params](const typename MT::ConstPtr& msg) { 
                (sensor_handle->*handler)(sensor_params, msg);
            }
        )
    );
};


SensorHandle::SensorHandle(std::shared_ptr<SensorConfig> config_ptr)
    :sensor_config_ptr_(config_ptr),image_transport_(nh_){
    // 发布静态TF变换
    publishSensorsStaticTF();
    // 初始化订阅者列表
    initSubscriber();
}

// 初始化订阅者
void SensorHandle::initSubscriber(){
    vector<SensorParams> params_list;
    // 订阅雷达数据
    params_list = sensor_config_ptr_->getLidarParams();
    for(auto& sensor_params : params_list){
        // 添加数据处理流程
        lidar_processor_list_[sensor_params.sensor_base.id] = LidarProcessor();
        // 订阅雷达数据
        LOG(INFO) << "subscribe lidar: " << sensor_params.sensor_base.topic_name;
        sub_list_[sensor_params.sensor_base.id] = SubscribeWithHandler<sensor_msgs::LaserScan>(
            &SensorHandle::HandleLaserScanMessage, sensor_params, &nh_, this);
    }

    // 订阅IMU数据
    params_list = sensor_config_ptr_->getIMUParams();
    for(auto& sensor_params : params_list){
        LOG(INFO) << "subscribe imu: " << sensor_params.sensor_base.topic_name;
        sub_list_[sensor_params.sensor_base.id] = SubscribeWithHandler<sensor_msgs::Imu>(
            &SensorHandle::HandleIMUMessage, sensor_params, &nh_, this);
    }

    // 订阅里程计信息
    SensorParams odom_sensor_params;
    odom_sensor_params.sensor_base.id = -100;
    odom_sensor_params.sensor_base.topic_name = "/odom";
    LOG(INFO) << "subscribe odom: " << odom_sensor_params.sensor_base.topic_name;
    sub_list_[odom_sensor_params.sensor_base.id] = SubscribeWithHandler<nav_msgs::Odometry>(
        &SensorHandle::HandleOdometryMessage, odom_sensor_params, &nh_, this);
    

    // 订阅深度相机数据
    params_list = sensor_config_ptr_->getDepthCameraParams();
    for(auto& sensor_params : params_list){
        // 添加数据处理流程
        depth_processor_list_[sensor_params.sensor_base.id] = DepthImageProcessor();
        // 深度图订阅
        LOG(INFO) << "subscribe depth_camera: " << sensor_params.sensor_base.topic_name;
        camera_sub_list_[sensor_params.sensor_base.id] = ImageSubscribeWithHandler<sensor_msgs::Image>(
            &SensorHandle::HandleCameraImageMessage, sensor_params, &image_transport_, this);
        // 深度图像信息订阅
        sensor_params.sensor_base.topic_name = sensor_params.sensor_base.camera_info_topic_name;
        LOG(INFO) << "subscribe depth_camera: " << sensor_params.sensor_base.topic_name;
        sub_list_[sensor_params.sensor_base.id] = SubscribeWithHandler<sensor_msgs::CameraInfo>(
            &SensorHandle::HandleCameraInfoMessage, sensor_params, &nh_, this);

    }

}


// 处理雷达类型消息
void SensorHandle::HandleLaserScanMessage(SensorParams sensor_params, const sensor_msgs::LaserScan::ConstPtr& msg){
    LaserScanData sensor_data;
    sensor_data.sensor_base = sensor_params.sensor_base;
    sensor_data.scan_ptr = boost::make_shared<sensor_msgs::LaserScan>(*msg);
    multi_lidar_cache_[sensor_params.sensor_base.id] = sensor_data;
}

// 处理IMU数据
void SensorHandle::HandleIMUMessage(SensorParams sensor_params, const sensor_msgs::Imu::ConstPtr& msg){
    imu_cache_ = boost::make_shared<sensor_msgs::Imu>(*msg);
}

// 处理里程计数据
void SensorHandle::HandleOdometryMessage(SensorParams sensor_params, const nav_msgs::Odometry::ConstPtr& msg){
    odom_cache_ = boost::make_shared<nav_msgs::Odometry>(*msg);
}

// 处理深度相机图像数据
void SensorHandle::HandleCameraImageMessage(SensorParams sensor_params, const sensor_msgs::Image::ConstPtr& msg){
    if(multi_image_cache_.count(sensor_params.sensor_base.id) == 0){
        ImageData sensor_data;
        sensor_data.sensor_base = sensor_params.sensor_base;
        sensor_data.image_ptr = boost::make_shared<sensor_msgs::Image>(*msg);
        multi_image_cache_[sensor_params.sensor_base.id] = sensor_data;
    }
    else{
        multi_image_cache_[sensor_params.sensor_base.id].image_ptr 
            = boost::make_shared<sensor_msgs::Image>(*msg);
    }    
}

// 处理深度相机信息数据
void SensorHandle::HandleCameraInfoMessage(SensorParams sensor_params, const sensor_msgs::CameraInfo::ConstPtr& msg){
    if(multi_image_cache_.count(sensor_params.sensor_base.id) == 0){
        ImageData sensor_data;
        sensor_data.sensor_base = sensor_params.sensor_base;
        sensor_data.camera_info_ptr = boost::make_shared<sensor_msgs::CameraInfo>(*msg);
        multi_image_cache_[sensor_params.sensor_base.id] = sensor_data;
    }
    else{
        multi_image_cache_[sensor_params.sensor_base.id].camera_info_ptr 
            = boost::make_shared<sensor_msgs::CameraInfo>(*msg);
    }
}

// 发布TF
void SensorHandle::publishStaticTF(geometry_msgs::TransformStamped transform_stamped){
    static_broadcaster_.sendTransform(transform_stamped);
}

void SensorHandle::publishStaticTF(string sensor_frame, SensorTF sensor_tf){
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = sensor_tf.base_frame;
    transformStamped.child_frame_id = sensor_frame;
    transformStamped.transform.translation.x = sensor_tf.x;
    transformStamped.transform.translation.y = sensor_tf.y;
    transformStamped.transform.translation.z = sensor_tf.z;

    tf2::Quaternion q;
    q.setRPY(sensor_tf.roll, sensor_tf.pitch, sensor_tf.yaw);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();
    publishStaticTF(transformStamped);
}

// 发布传感器静态TF变换
void SensorHandle::publishSensorsStaticTF(){
    vector<SensorParams> params_list;
    // 激光雷达静态变换数据
    auto lidar_params_list = sensor_config_ptr_->getLidarParams();
    params_list.insert(params_list.end(), lidar_params_list.begin(), lidar_params_list.end());
    // IMU静态变换数据
    auto imu_params_list = sensor_config_ptr_->getIMUParams();
    params_list.insert(params_list.end(), imu_params_list.begin(), imu_params_list.end());
    // 深度相机静态变换数据
    auto depth_params_list = sensor_config_ptr_->getDepthCameraParams();
    params_list.insert(params_list.end(), depth_params_list.begin(), depth_params_list.end());

    for(auto& sensor_params : params_list){
        publishStaticTF(sensor_params.sensor_base.sensor_frame, sensor_params.transform);
    }

    // 发布base_link -> base_footprint
    double wheel_radius = ros::param::param<double>("/wheel_radius", 0.07);
    SensorTF sensor_tf;
    sensor_tf.base_frame = "base_link";
    sensor_tf.z = wheel_radius;
    publishStaticTF("base_footprint", sensor_tf);
}

// 处理深度相机数据
vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> SensorHandle::DepthCameraHandle(){
    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> pointcloud_list;
    RobotModel robot_model = sensor_config_ptr_->getRobotModel();
    for(auto iter = depth_processor_list_.begin(); iter != depth_processor_list_.end(); iter++){
        // LOG(INFO) << "id: " << iter->first << endl;
        // 获取深度数据
        if(multi_image_cache_.count(iter->first) == 0) continue;
        DepthImageProcessor& depth_processor = iter->second;

        // 初始化操作
        ImageData& image_data = multi_image_cache_[iter->first];
        if(!depth_processor.depth2pointcloud.isInit()){
            if(image_data.camera_info_ptr)
                depth_processor.depth2pointcloud.setCameraInfo(image_data.camera_info_ptr);
            vector<SensorParams> params_list = sensor_config_ptr_->getDepthCameraParams();
            for(auto& sensor_params : params_list){
                if(sensor_params.sensor_base.id == iter->first){
                    depth_processor.depth2pointcloud.setParams(sensor_params);
                    break;
                }
            }
            continue;   //如果此次设置成功，下载不再重新设置；如果此次设置失败，将循环报错
        }
        SensorParams sensor_params = depth_processor.depth2pointcloud.getParams();
        if(!depth_processor.visualization.isDepthCameraInit()){
            depth_processor.visualization.initDepth2PointCloudPublish(
                "/depth_pointcloud_" + to_string(sensor_params.sensor_base.id));
        }
        if(!image_data.image_ptr) continue;

        // 深度转点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_raw_ptr 
            = depth_processor.depth2pointcloud.getPointCloud(image_data.image_ptr);
        
        // 发布原始点云数据 base_footprint坐标系下
        sensor_msgs::PointCloud2::Ptr pointcloud_msg_ptr =
            msg_convert::toPointCloud2(pointcloud_raw_ptr, "base_footprint");
        depth_processor.visualization.publishDepthPointCloudRaw(pointcloud_msg_ptr);

        // 数据过滤
        depth_processor.pcl_filter.voxelFilter(pointcloud_raw_ptr, 0.02f, 0.02f, 0.02f);
        SensorFilterConfig filter_config = sensor_config_ptr_->getSensorFilterConfig();
        depth_processor.pcl_filter.filterAxis(pointcloud_raw_ptr, "z", filter_config.top_limit, 100.0f);
        depth_processor.pcl_filter.filterAxis(pointcloud_raw_ptr, "z", -100.0f, filter_config.botton_limit);
        depth_processor.pcl_filter.filterAxis(pointcloud_raw_ptr, "z", filter_config.ground_below, filter_config.ground_above);

        vector<pair<double, double>> polygon;
        for(auto point : robot_model.robot_footprints){
            polygon.emplace_back(point.x, point.y);
        }
        // polygon.emplace_back(0.35, 0.3);
        // polygon.emplace_back(-0.35, 0.3);
        // polygon.emplace_back(-0.35, -0.3);
        // polygon.emplace_back(0.35, -0.3);
        depth_processor.pcl_filter.PolygonFilter(pointcloud_raw_ptr, polygon);

        // 发布处理后点云数据 base_footprint坐标系下
        pointcloud_msg_ptr = msg_convert::toPointCloud2(pointcloud_raw_ptr, "base_footprint");
        depth_processor.visualization.publishDepthPointCloud(pointcloud_msg_ptr);  
        
        pointcloud_list.emplace_back(pointcloud_raw_ptr);
    }
    return pointcloud_list;
}

// 处理激光雷达数据
vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> SensorHandle::Lidar2DHandle(){
    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> pointcloud_list;
    RobotModel robot_model = sensor_config_ptr_->getRobotModel();
    for(auto iter = lidar_processor_list_.begin(); iter != lidar_processor_list_.end(); iter++){
        // LOG(INFO) << "id: " << iter->first << endl;
        // 获取深度数据
        if(multi_lidar_cache_.count(iter->first) == 0) continue;
        LidarProcessor& lidar_processor = iter->second;

        // 初始化操作
        LaserScanData& lidar_data = multi_lidar_cache_[iter->first];
        if(!lidar_processor.visualization.isScanInit()){
            lidar_processor.visualization.initScan2PointCloudPublish(
                "/scan_pointcloud_" + to_string(iter->first));
        }
        if(!lidar_processor.lidar_2d_undistortion.isInit()){
            vector<SensorParams> params_list = sensor_config_ptr_->getLidarParams();
            for(auto& sensor_params : params_list){
                if(sensor_params.sensor_base.id == iter->first){
                    lidar_processor.lidar_2d_undistortion.setParams(sensor_params);
                    break;
                }
            }
            continue;   //如果此次设置成功，下载不再重新设置；如果此次设置失败，将循环报错
        }
        if(!lidar_data.scan_ptr) continue;

        // 去除运动畸变

        // 转换成点云数据
        pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_raw_ptr =
            msg_convert::toPointCloudXYZ(lidar_data.scan_ptr);
        pointcloud_raw_ptr = lidar_processor.lidar_2d_undistortion.transformPCL(pointcloud_raw_ptr);
        
        // 发布原始点云数据 base_link坐标系下
        sensor_msgs::PointCloud2::Ptr pointcloud_msg_ptr =
            msg_convert::toPointCloud2(pointcloud_raw_ptr, "base_link");
        
        lidar_processor.visualization.publishScanPointCloudRaw(pointcloud_msg_ptr);        

        // 数据过滤
        lidar_processor.pcl_filter.voxelFilter(pointcloud_raw_ptr, 0.01f, 0.01f, 0.01f);
        vector<pair<double, double>> polygon;
        for(auto point : robot_model.robot_footprints){
            polygon.emplace_back(point.x, point.y);
        }
        // polygon.emplace_back(0.35, 0.35);
        // polygon.emplace_back(-0.35, 0.35);
        // polygon.emplace_back(-0.35, -0.35);
        // polygon.emplace_back(0.35, -0.35);
        lidar_processor.pcl_filter.PolygonFilter(pointcloud_raw_ptr, polygon);

        // 发布处理后点云数据 base_link坐标系下
        pointcloud_msg_ptr = msg_convert::toPointCloud2(pointcloud_raw_ptr, "base_link");
        lidar_processor.visualization.publishScanPointCloud(pointcloud_msg_ptr);
        
        pointcloud_list.emplace_back(pointcloud_raw_ptr);
    }
    return pointcloud_list;
}

// 点云融合
pcl::PointCloud<pcl::PointXYZ>::Ptr SensorHandle::fusionPointCloud(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds){
    pcl::PointCloud<pcl::PointXYZ>::Ptr fused_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 点云拼接
    for(auto& cloud : clouds){
        // if(clouds[i]->empty()) continue;
        *fused_cloud += *cloud;
    }
    return fused_cloud;
}

// 点云投影
void SensorHandle::projectPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud){
    pcl_filter_.projectionXY(cloud);
}